#include "truck.h"
#include <cmath>
#include <sstream>
#include <iostream>
#include <cassert>
#include "loadArea.h"
#include "tire.h"
#include "crate.h"

Truck::Truck(const Cube& chasis,const Cube& loadArea,const Point& dir,Path* path,const ObjectModelPtr& chasisModel,const ObjectModelPtr& tireModel,const ObjectModelPtr& loadAreaModel)
	:	SceneObject(chasis,dir,chasisModel),
		Solid( chasis ){
	this->maxSpeed=0;
	this->minSpeed=0;
	this->accel=0;
	this->zRotatedAngle=0;
	this->xRotatedAngle=0;
	this->zDiffRotated=0;
	this->path=path;
	this->stopped=true;
	this->loaded=false;


	Cube loadAreaScaled = Cube( Point(loadArea.minX()*chasis.width(),loadArea.minY()*chasis.depth(),loadArea.minZ()*chasis.height()),
								Point(loadArea.maxX()*chasis.width(),loadArea.maxY()*chasis.depth(),loadArea.maxZ()*chasis.height()));
	this->setSize(loadAreaScaled.translate(chasis.center().x1,chasis.center().x2,chasis.center().x3));

	// LoadArea
	LoadArea* larea = new LoadArea(loadArea,loadAreaModel);
	this->addObject(LoadArea::name(),larea);
	larea->setParent(this);

	// Iterator
	this->iterator = this->path->createPathIterator(1,true);
	this->iterator->addFollower(this);
	getIteratorData();

	//Tires
	Cube tireCubes[4] = { 	Cube(Point(0.16f+0.08,-0.005f-0.2f,-0.2f)  ,Point(0.08f,-0.155f-0.2f,-0.05f)),
							Cube(Point(-0.16f-0.08f,-0.005f-0.2f,-0.2f)  ,Point(-0.08f,-0.155f-0.2f,-0.05f)),
							Cube(Point(-0.16f-0.08f,0.305f-0.05f,-0.2f)  ,Point(-0.08f,0.455f-0.05f,-0.05f)),
							Cube(Point(0.16f+0.08f,0.305f-0.05f,-0.2f)  ,Point(0.08f,0.455f-0.05f,-0.05f)) };

	for(unsigned int c=0;c<4;c++){
		Tire* tire = new Tire(tireCubes[c],tireModel);
		tire->setParent(this);

		std::ostringstream stream;
		stream << Tire::name() << c;
		this->addObject(stream.str(),tire);
	}
}

Truck::~Truck(){
	delete iterator;
	delete path;

	if(loaded)
		this->getObject(Crate::name());
}

void Truck::setModelMatrix(){
	Point tr = this->dimensions.center();
	glTranslatef(tr.x1,tr.x2,tr.x3-0.7f);
	glRotatef(this->zRotatedAngle*180/PI,0.0f,0.0f,1.0f);
	glRotatef(this->xRotatedAngle*180/PI,1.0f,0.0f,0.0f);
	glScalef(this->dimensions.width(),this->dimensions.depth(),this->dimensions.height());
	glRotatef(-90,0,0,1);
}

void Truck::setSubModelMatrix(const std::string& name){
	if(name!=Crate::name()){
		Point tr = this->dimensions.center();
		glTranslatef(tr.x1,tr.x2,tr.x3-0.7f);
		glRotatef(this->zRotatedAngle*180/PI,0.0f,0.0f,1.0f);
		glRotatef(this->xRotatedAngle*180/PI,1.0f,0.0f,0.0f);
		glScalef(this->dimensions.width(),this->dimensions.depth(),this->dimensions.height());
	}else{
		Point crateCenter = this->findObject(Crate::name()).getDimensions().center();
		Point tr = crateCenter-this->dimensions.center();
		glRotatef(this->zRotatedAngle*180/PI,0,0,1);
		glRotatef(this->xRotatedAngle*180/PI,1.0f,0.0f,0.0f);
		glTranslatef(tr.x1,tr.x2-this->getDimensions().depth()/3,tr.x3+this->findObject(Crate::name()).getDimensions().height()/2-0.7f);
	}
}

void Truck::timeStep(){
	if(stopped)
		return;

	if(!iterator->end() || !iterator->begin()){
		PathData initial = this->iterator->getInitial();
		PathData last = this->iterator->getLast();

		this->speed += this->getDirection()*this->accel;

		if(this->speed.mod()>this->maxSpeed)
			this->speed = this->getDirection()*this->maxSpeed;

		if(this->speed.mod()<this->minSpeed || (this->speed*this->direction<0))
			this->speed = this->direction*this->minSpeed;


		// Se freno por completo antes de llegar
		if(this->speed*this->getDirection()<0 && this->accel<0){
			this->speed = Point(0,0,0);
			Point center = last.p;
			center.x3 = this->dimensions.center().x3;

			Point calc = this->dimensions.center()-center;
			this->move(calc.x1,calc.x2,calc.x3);
		}


		this->dimensions.advance(this->speed);
		this->size.advance(this->speed);

		// Utilizo como plano (initial-last)(normal)
		// El producto vectorial indica de que lado del plano esta,
		// si esta del lado negativo se paso, se debe reacomodar y avanzar a
		// los otros 2 puntos.

		Point center = this->dimensions.center();
		if( (center-last.p)*(initial.p-last.p) <= 0){
			center = last;
			Point calc = center-this->dimensions.center();
			this->move(calc.x1,calc.x2,calc.x3);

			++(*this->iterator);
		}
	}

	for(unsigned int c=0;c<4;c++){
		std::ostringstream stream;
		stream << Tire::name() << c;
		this->findObject(stream.str()).setSpeed(this->speed);
		this->findObject(stream.str()).timeStep();
	}

	if(loaded){
		this->findObject(Crate::name()).setParent(this);
		Point diff = this->dimensions.center() - this->findObject(Crate::name()).getDimensions().center();
		this->findObject(Crate::name()).move(diff.x1,diff.x2,diff.x3);
	}
}

void Truck::updatePosition(const Point& position,const Point& direction,PathInfo* info){
	getIteratorData();
}

void Truck::getIteratorData(){
	PathData initial = iterator->getInitial();
	PathData last = iterator->getLast();

	TruckPathInfo* info = static_cast<TruckPathInfo*>(initial.info);
	this->accel = info->getAccel();
	this->maxSpeed = info->getMaxSpeed();
	this->minSpeed = info->getMinSpeed();

	info->pathNotifyCallback(NULL);

	Point oldDir = this->getDirection();
	this->setDirection(last.p-initial.p);
	this->speed=this->getDirection()*this->speed.mod();

	// zRotatedAngle, Proyecto en xy
	float zAng1 	= atan2(oldDir.x1,oldDir.x2);
	float zAng2 	= atan2(this->getDirection().x1,this->getDirection().x2);
	this->zDiffRotated = zAng1-zAng2;

	this->zRotatedAngle += this->zDiffRotated;

	if(this->zRotatedAngle>=2*PI)
		this->zRotatedAngle -= 2*PI;
	else if(this->zRotatedAngle<(-2*PI))
		this->zRotatedAngle += 2*PI;

	// xRotatedAngle
	float xAng1 = atan2(oldDir.x3,sqrt(pow(oldDir.x1,2)+pow(oldDir.x2,2)));
	float xAng2 = atan2(this->getDirection().x3,sqrt(pow(this->getDirection().x1,2)+pow(this->getDirection().x2,2)));
	this->xDiffRotated = xAng2-xAng1;

	this->xRotatedAngle += this->xDiffRotated;

	if(this->xRotatedAngle>=2*PI)
		this->xRotatedAngle -= 2*PI;
	else if(this->xRotatedAngle<(-2*PI))
		this->xRotatedAngle += 2*PI;
}


void Truck::notifyCollision(Solid* obj){
	Crate* crate = dynamic_cast<Crate*>(obj);

	// No es una caja
	if(crate==NULL)
		return;

	this->addObject(Crate::name(),crate);
	this->stopped=false;
	this->loaded=true;
}

void Truck::notifySolidPositionChange(float vel){
	//this->dimensions.translate(0,0,vel);
}

void Truck::unloadAreaReached(void*){
	this->getObject(Crate::name())->setParent(NULL);
	this->loaded=false;
}

void Truck::loadAreaReached(void*){
	this->stopped=true;
	// TODO fix temporal -> ver porque se arruina la coordenada z en el recorido
	Point lcenter = this->size.center();
	this->size.center(Point(lcenter.x1,lcenter.x2,0.6f));
}

void Truck::move(float dx,float dy,float dz){
	this->dimensions.translate(dx,dy,dz);
	this->size.translate(dx,dy,dz);
}

Point Truck::getDriverPosition(){
	Point norm = this->direction.cross(Point(0,0,-1));
	return this->dimensions.center() + this->direction*0.2f + norm*0.6f;
}
